#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/features/feature.h>
#include <Eigen/Eigen>
#include <geometry_msgs/Vector3Stamped.h>


ros::Publisher pub;
ros::Publisher pub2;

double previousMax = 0;
Eigen::Vector3f previousColourFloor;
geometry_msgs::Vector3Stamped colourFloorMsg;

//geometry_msgs::Vector3 colourFloorMsg;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& msg)
{
    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr outCloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg (*msg, *cloud);    // Publish the data

    double max = -10000;
    double colorR = 0;
    double colorG = 0;
    double colorB = 0;
    int numberOfFloorPixels = 0;

    BOOST_FOREACH(pcl::PointXYZRGB point, *cloud)
    {
        if( point.x != NAN || point.y != NAN || point.z != NAN)
        {
            if(point.y > max) max = point.y;
        }
    }

    if(previousMax != 0){
        previousMax = (max*0.1) + (previousMax*0.9);
    }
    else
    {
        previousMax = max;
    }

    BOOST_FOREACH(pcl::PointXYZRGB point, *cloud)
    {
        if(point.y < previousMax-0.4 && point.z < 6.5 && point.x != NAN && point.y != NAN  && point.z != NAN )
        {
            outCloud->push_back(point);
        }
        else if(point.x != NAN && point.y != NAN  && point.z != NAN && point.z < 6.5)
        {
            if (point.getRGBVector3i()[0] != NAN && point.getRGBVector3i()[1] != NAN && point.getRGBVector3i()[2] != NAN)
            {
                numberOfFloorPixels ++;
                colorR += point.getRGBVector3i()[0];
                colorG += point.getRGBVector3i()[1];
                colorB += point.getRGBVector3i()[2];
            }
        }
    }

    // Calculate colour of floor
    Eigen::Vector3f colourFloor;

    if(!numberOfFloorPixels < 1)
    {
        colourFloor[0] = colorR / numberOfFloorPixels;
        colourFloor[1] = colorG / numberOfFloorPixels;
        colourFloor[2] = colorB / numberOfFloorPixels;


        if(previousColourFloor[0] != 0)
        {
            previousColourFloor[0] = previousColourFloor[0]*0.9 + colourFloor[0]*0.1;
            previousColourFloor[1] = previousColourFloor[1]*0.9 + colourFloor[1]*0.1;
            previousColourFloor[2] = previousColourFloor[2]*0.9 + colourFloor[2]*0.1;
        }
        else
        {
            previousColourFloor = colourFloor;
        }

        std::cout << previousColourFloor << std::endl;
        std::cout << "----------" << std::endl;

        // Store data in ros msg
        pcl::toROSMsg(*outCloud, output);
        output.fields = msg->fields;
        output.header = msg->header;

        colourFloorMsg.header = msg->header;
        colourFloorMsg.vector.x = previousColourFloor[0];
        colourFloorMsg.vector.y = previousColourFloor[1];
        colourFloorMsg.vector.z = previousColourFloor[2];

        // publish results
        pub.publish(output);
        pub2.publish(colourFloorMsg);
    }
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("/godot/object_segmentation", 1);
  pub2 = nh.advertise<geometry_msgs::Vector3Stamped> ("/godot/colour_floor", 1);

  // Spin
  ros::spin ();
}
